package prototype2;

import java.awt.Color;
import java.awt.Dimension;
import java.awt.Font;
import java.awt.Graphics2D;
import java.awt.Image;
import java.awt.Point;
import java.awt.Rectangle;
import java.awt.RenderingHints;
import java.awt.event.MouseEvent;
import java.awt.event.MouseListener;
import java.awt.geom.Ellipse2D;
import java.awt.geom.Line2D;
import java.awt.geom.Point2D;
import java.awt.geom.Rectangle2D;
import java.util.ArrayList;

import javax.swing.JFrame;

public class PotentialFieldsMain implements MouseListener {

	private JFrame mainFrame = null;
	private SimpleReactiveRobot robot;

	public void setGoalPosition(double x, double y) {
		robot.goal.x = x;
		robot.goal.y = y;
	}

	public void init() {
		mainFrame = new JFrame("Potential fields demo");
		mainFrame.setBounds(new Rectangle(new Point(10, 10), new Dimension(600,
				600)));
		mainFrame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
		mainFrame.setVisible(true);
		((Graphics2D) mainFrame.getGraphics()).setRenderingHint(
				RenderingHints.KEY_ANTIALIASING,
				RenderingHints.VALUE_ANTIALIAS_ON);
		mainFrame.setBackground(Color.white);
		mainFrame.getGraphics().setColor(Color.red);
		mainFrame.addMouseListener(this);
	}

	public void drawRobot(SimpleReactiveRobot robot) {
		Image offscreen = mainFrame.createImage(mainFrame.getWidth(), mainFrame
				.getHeight());
		Graphics2D g2 = (Graphics2D) offscreen.getGraphics();
		g2.clearRect(0, 0, 600, 600);
		g2.draw(new Ellipse2D.Double(robot.position.getX() - 5, robot.position
				.getY() - 5, 10, 10));
		g2.setColor(Color.GREEN);
		g2.drawLine((int) (robot.position.getX()),
				(int) (robot.position.getY()),
				(int) ((robot.position.getX()) + (robot.speed * 10 * Math
						.cos(robot.orientation))), (int) ((robot.position
						.getY()) + (robot.speed * 10 * Math
						.sin(robot.orientation))));
		double dhalf;
		for (prototype2.PotentialFieldsMain.SimpleReactiveRobot.Sensor s : robot.sensors) {
			g2.setColor(Color.red);
			dhalf = s.sensorRange;
			g2.draw(new Ellipse2D.Double(robot.position.getX() - dhalf,
					robot.position.getY() - dhalf, 2 * s.sensorRange,
					2 * s.sensorRange));
		}

		for (SimpleWorldObject swo : robot.objects) {
			g2.setColor(Color.blue);
			g2.setFont(new Font("Courier", 0, 10));
			dhalf = swo.size;
			g2.draw(new Ellipse2D.Double(swo.x - dhalf, swo.y - dhalf,
					2 * swo.size, 2 * swo.size));
			g2.drawString(swo.field.flavour.getMark(), (float) swo.x - 3,
					(float) swo.y + 3);
		}

		g2.fill(new Rectangle2D.Double(robot.goal.x - 5, robot.goal.y - 5, 10,
				10));
		mainFrame.getGraphics().drawImage(offscreen, 0, 0, mainFrame);
	}

	/**
	 * @param args
	 */
	public static void main(String[] args) {
		Line2D.Double line = new Line2D.Double(0, 0, 200, 200);
		Rectangle2D.Double rect = new Rectangle2D.Double(160, 60, 40, 40);

		PotentialFieldsMain pfm = new PotentialFieldsMain();
		pfm.init();

		ArrayList<SimpleWorldObject> objects = new ArrayList<SimpleWorldObject>();
		SimpleWorldObject obj1 = pfm.new SimpleWorldObject(200, 200, 100,
				PotentialFieldType.LINEAR, PotentialFieldFlavour.REPULSION);
		SimpleWorldObject obj2 = pfm.new SimpleWorldObject(200, 400, 100,
				PotentialFieldType.LINEAR, PotentialFieldFlavour.REPULSION);
		SimpleWorldObject obj3 = pfm.new SimpleWorldObject(400, 200, 100,
				PotentialFieldType.LINEAR, PotentialFieldFlavour.REPULSION);
		SimpleWorldObject obj4 = pfm.new SimpleWorldObject(400, 400, 100,
				PotentialFieldType.LINEAR, PotentialFieldFlavour.REPULSION);
		SimpleWorldObject obj5 = pfm.new SimpleWorldObject(200, 200, 100,
				PotentialFieldType.LINEAR, PotentialFieldFlavour.ATTRACTION);
		SimpleWorldObject obj6 = pfm.new SimpleWorldObject(200, 400, 100,
				PotentialFieldType.LINEAR, PotentialFieldFlavour.ATTRACTION);
		SimpleWorldObject obj7 = pfm.new SimpleWorldObject(400, 200, 100,
				PotentialFieldType.LINEAR, PotentialFieldFlavour.ATTRACTION);
		SimpleWorldObject obj8 = pfm.new SimpleWorldObject(400, 400, 100,
				PotentialFieldType.LINEAR, PotentialFieldFlavour.ATTRACTION);
		// objects.add(obj1);
		// objects.add(obj2);
		// objects.add(obj3);
		// objects.add(obj4);
		// objects.add(obj5);
		// objects.add(obj6);
		// objects.add(obj7);
		// objects.add(obj8);

		// / simple setting
		SimpleWorldObject obj9 = pfm.new SimpleWorldObject(200, 210, 100,
				PotentialFieldType.LINEAR, PotentialFieldFlavour.TANGENTIAL);
		SimpleWorldObject obj10 = pfm.new SimpleWorldObject(200, 440, 100,
				PotentialFieldType.LINEAR, PotentialFieldFlavour.REPULSION);
		SimpleWorldObject obj11 = pfm.new SimpleWorldObject(400, 400, 100,
				PotentialFieldType.LINEAR, PotentialFieldFlavour.ATTRACTION);
		objects.add(obj9);
		objects.add(obj10);
		objects.add(obj11);

		pfm.robot = pfm.new SimpleReactiveRobot(100, 300, objects);
		SimpleReactiveRobot robot = pfm.robot;

		pfm.setGoalPosition(300, 300);

		pfm.drawRobot(robot);

		// testing section
		while (true) {
			if (!new Rectangle2D.Double(robot.goal.x - 5, robot.goal.y - 5, 10,
					10).contains(robot.position)) {
				// robot.orientation += Math.PI / 90;
				robot.position.setLocation(new Point2D.Double(robot.position
						.getX()
						+ robot.speed / 5.0 * (Math.cos(robot.orientation)),
						robot.position.getY() + robot.speed / 5.0
								* Math.sin(robot.orientation)));

				// pfm.drawRobot(robot);
				robot.sense();
				// System.out.println(robot.orientation);
				try {
					Thread.sleep(10);
				} catch (InterruptedException e) {
					e.printStackTrace();
				}
			}
			pfm.drawRobot(robot);
		}
	}

	private class SimpleReactiveRobot {
		private Point2D position;
		private double orientation;
		private double speed;
		private ArrayList<SimpleWorldObject> objects;
		private Goal goal;

		private ArrayList<Sensor> sensors;
		boolean anealingMode = false;
		int amCounter = 0;

		public SimpleReactiveRobot(double x, double y,
				ArrayList<SimpleWorldObject> objects) {

			position = new Point2D.Double(x, y);
			orientation = 0.0;
			speed = 8.0;
			sensors = new ArrayList<Sensor>();
			this.objects = objects;
			sensors.add(new Sensor(this, 50));
			goal = new Goal(300, 200);
		}

		public void sense() {
			DirectionVector v = new DirectionVector();
			for (Sensor s : sensors) {
				v.addDirectionVector(s.sense());
			}
			v.addDirectionVector(goal.getV());
			this.orientation = v.getOrientation();
			this.speed = v.getIntensity();
			if (Math.abs(this.speed) < 1) {
				anealingMode = true;
				amCounter = 0;
			}
			if (anealingMode) {
				v.addDirectionVector(Util.makeIntensive(goal.getV()));
				++amCounter;
				if (amCounter == 5)
					anealingMode = false;
			}
			this.orientation = v.getOrientation();
			this.speed = v.getIntensity();
		}

		private class Goal {
			public Goal(double x, double y) {
				this.x = x;
				this.y = y;
				v = new DirectionVector();
			}

			private double x, y;
			private DirectionVector v;

			public DirectionVector getV() {
				double o;
				o = Util.calculateOrientation(position.getX(), position.getY(),
						this.x, this.y, PotentialFieldFlavour.ATTRACTION);
				v = new DirectionVector(2, o);
				return v;
			}
		}

		private class Sensor {

			private ArrayList<SimpleWorldObject> objects;
			public double sensorRange;
			public SimpleReactiveRobot robot;

			public Sensor(SimpleReactiveRobot robot, double sensorRange) {
				this.objects = robot.objects;
				this.sensorRange = sensorRange;
				this.robot = robot;
			}

			public DirectionVector sense() {
				double retval = 0.0;
				double orient = 0.0;
				double dhalf1, dhalf2;
				Ellipse2D.Double ellipse1, ellipse2;
				DirectionVector d = new DirectionVector();
				for (SimpleWorldObject swo : this.objects) {
					dhalf1 = sensorRange;
					dhalf2 = swo.size;
					ellipse1 = new Ellipse2D.Double(robot.position.getX()
							- dhalf1, robot.position.getY() - dhalf1,
							2 * sensorRange, 2 * sensorRange);
					ellipse2 = new Ellipse2D.Double(swo.x - dhalf2, swo.y
							- dhalf2, 2 * swo.size, 2 * swo.size);
					if (Util.intersects(ellipse1, ellipse2)) {
						// System.out.println("true");
						retval = Util.calculateIntensity(robot.position.getX(),
								robot.position.getY(), swo.x, swo.y,
								swo.field.type, sensorRange, swo.size,
								swo.field.flavour);
						if (swo.field.flavour == PotentialFieldFlavour.TANGENTIAL)
							retval *= 3;
						orient = Util.calculateOrientation(robot.position
								.getX(), robot.position.getY(), swo.x, swo.y,
								swo.field.flavour);
						 System.out.println("orient : " + orient);
						d
								.addDirectionVector(new DirectionVector(retval,
										orient));
					}
				}
				// System.out.println(d.getIntensity() + " - "
				// + d.getOrientation());
				return d;
			}
		}
	}

	private class SimpleWorldObject {
		PotentialField field;
		double x, y;
		double size;

		public SimpleWorldObject(double x, double y, double size,
				PotentialFieldType type, PotentialFieldFlavour flavour) {
			this.x = x;
			this.y = y;
			this.size = size;
			field = new PotentialField(type, flavour);
		}

		private class PotentialField {

			public PotentialField(PotentialFieldType type,
					PotentialFieldFlavour flavour) {
				this.type = type;
				this.flavour = flavour;
			}

			private PotentialFieldType type;
			private PotentialFieldFlavour flavour;
		}
	}

	@Override
	public void mouseClicked(MouseEvent e) {

	}

	@Override
	public void mouseEntered(MouseEvent e) {
		// TODO Auto-generated method stub

	}

	@Override
	public void mouseExited(MouseEvent e) {
		// TODO Auto-generated method stub

	}

	@Override
	public void mousePressed(MouseEvent e) {
		this.setGoalPosition(e.getX(), e.getY());

	}

	@Override
	public void mouseReleased(MouseEvent e) {
		// TODO Auto-generated method stub

	}
}

class Util {

	public static DirectionVector makeIntensive(DirectionVector v) {
		return new DirectionVector(v.getIntensity() * 10, v.getOrientation());
	}

	public static boolean intersects(Ellipse2D first, Ellipse2D second) {
		return distance(first, second) <= 0;
	}

	public static double calculateOrientation(double x1, double y1, double x2,
			double y2, PotentialFieldFlavour flavour) {
		if (x2 - x1 != 0) {
			double o = Math.atan((y2 - y1) / (x2 - x1));
			if (PotentialFieldFlavour.ATTRACTION == flavour) {
				return fixOrientForAttraction(o, x1, y1, x2, y2);
			}
			if (PotentialFieldFlavour.REPULSION == flavour) {
				return fixOrientForRepulsion(o, x1, y1, x2, y2);
			}
			if (PotentialFieldFlavour.TANGENTIAL == flavour) {
				return fixOrientForTangential(o, x1, y1, x2, y2);
			}
		}
		return 0.0;
	}

	private static double fixOrientForTangential(double o, double x1,
			double y1, double x2, double y2) {
		return fixOrientForRepulsion(o, x1, y1, x2, y2) - Math.PI/3;
	}

	private static double fixOrientForRepulsion(double o, double x1, double y1,
			double x2, double y2) {
		// System.out.println(x1 + " - " + y1 + " - " + x2 + " - " + y2);
		if ((x2 < x1) && (y2 < y1)) {
			return o;
		}
		if ((x2 < x1) && (y2 > y1)) {
			return o;
		}
		if ((x2 > x1) && (y2 > y1)) {
			return o - Math.PI;
		}
		// System.out.println(o - Math.PI);
		return o - Math.PI;
	}

	private static double fixOrientForAttraction(double o, double x1,
			double y1, double x2, double y2) {
		if ((x2 < x1) && (y2 < y1))
			return o - Math.PI;
		if ((x2 < x1) && (y2 > y1))
			return o - Math.PI;
		if ((x2 > x1) && (y2 > y1))
			return o;
		return o;
	}

	public static double calculateIntensity(double x1, double y1, double x2,
			double y2, PotentialFieldType type, double sensorRange,
			double objectSize, PotentialFieldFlavour flavour) {
		double d = distance(x1, y1, x2, y2, objectSize, sensorRange);
		if (d != 0) {
			if (type == PotentialFieldType.CONSTANT) {
				return 1.0;
			}
			if (type == PotentialFieldType.LINEAR) {
				if (PotentialFieldFlavour.ATTRACTION == flavour) {
					return (sensorRange / (sensorRange - d) > 5) ? 5
							: sensorRange / (sensorRange - d);
				}
				if (PotentialFieldFlavour.REPULSION == flavour)
					return sensorRange / (sensorRange + d);
				if (PotentialFieldFlavour.TANGENTIAL == flavour)
					return sensorRange / (sensorRange + d);
			}
			if (type == PotentialFieldType.EXPONENTIAL)
				return 5 / Math.exp(d / 500);
		}
		return 0.0;
	}

	private static double distance(Ellipse2D first, Ellipse2D second) {
		return (Math.sqrt((first.getCenterX() - second.getCenterX())
				* (first.getCenterX() - second.getCenterX())
				+ ((first.getCenterY() - second.getCenterY()) * (first
						.getCenterY() - second.getCenterY()))))
				- first.getWidth() / 2 - second.getWidth() / 2;
	}

	private static double distance(double x1, double y1, double x2, double y2,
			double objectSize, double sensorRange) {
		if (Math.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2))
				- objectSize > 0) {
			// System.out.println(Math.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2)
			// * (y1 - y2))
			// - objectSize);
			return Math.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2))
					- objectSize;
		}
		return 0.0;
	}
}

enum PotentialFieldFlavour {

	UNIFORM("U"), PERPENDICULAR("P"), ATTRACTION("A"), REPULSION("R"), TANGENTIAL(
			"T");

	PotentialFieldFlavour(String s) {
		mark = s;
	}

	private String mark;

	public String getMark() {
		return mark;
	}
}

enum PotentialFieldType {
	CONSTANT("C"), LINEAR("L"), EXPONENTIAL("E");

	PotentialFieldType(String s) {
		mark = s;
	}

	private String mark;

	public String getMark() {
		return mark;
	}
}